#include <cv.h>
#include <cxcore.h>
#include <highgui.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <queue>
#include <vector>
#include "tracker.h"
#include "buffer.h"



#ifdef UNIT_TEST
int num_car_enter;
int num_car_exit;
int num_car_park;
int num_car_unpark;
#endif

using namespace std;

bool delay = false;

SocketClient *Top_Cam_Sock;


//list<car*> list_of_cars;

vector <car> list_of_cars;
//#define CALEBRATE_X_Y
/*
 * #define HSV_DEBUG		//Activate the printf line to debug the HSV transform calculation
*/


/*OPTIMIZATION OPTIONS
 * Use the drected vector of the point that we found as the center of gravity.
 */

bool isThisParkingSlot(CvPoint *PointArr) {
	if ( ( PointArr->x >= PARK_LEFT_UP.x ) && ( PointArr->x <= PARK_RIGHT_DOWN.x ) &&
		( PointArr->y >= PARK_RIGHT_DOWN.y ) && ( PointArr->y <= PARK_LEFT_UP.y ) ) return true;
	return false;
}

/*
 * Retrieves the maximum of 3 ints.
 */
int maxNum(uchar x, uchar y, uchar z) {
	int max =x;
	if (y > max) max = y;
	if (z > max) max = z;
	return max;
}

/*
 * Retrieves the minimum of 3 ints.
 */
int minNum(uchar x, uchar y, uchar z) {
	int min =x;
	if (y < min) min = y;
	if (z < min) min = z;
	return min;
}
uchar* copyData(uchar *data,int height,int width,int channels,int step){
    uchar* new_data = new uchar[height*width*channels];
	for (int y=0; y < height; ++y) 
		for (int x=0; x < width; ++x) {
			new_data[y*step+x*channels] = data[y*step+x*channels+0];
			new_data[y*step+x*channels+1] = data[y*step+x*channels+1];
			new_data[y*step+x*channels+2] = data[y*step+x*channels+2] ;
	}
	return new_data;
}
bool carArrived( CvPoint center, int lp )
{
	car tmp;
	tmp.p= center;
	tmp.licensePlate = lp;
	list_of_cars.push_back( tmp );
	Protocol::tc2db_enetered (*Top_Cam_Sock , (unsigned long) lp );
	return true;		
}

/*
 * This function transform the frame from RGB to HSV.
 * Return Value: An array of int that holds the values for HSV.
 */
int* ConvertFromRgbToHsv(IplImage* img){
	int height = img->height;
	int width = img->width;
	int step = img->widthStep;
	int channels  = img->nChannels;
	uchar *data = (uchar *)img->imageData;
	uchar blue,green,red;
	int h=0,s,v,max,min;
	int* new_data = new int[height*width*channels];
	for (int y=0; y < height; ++y) {
		for (int x=0; x < width; ++x) {
			blue = data[y*step+x*channels+0];
			green = data[y*step+x*channels+1];
			red = data[y*step+x*channels+2] ;
			max = maxNum(blue,green,red);
			min = minNum(blue,green,red);
// V calculation
			v= max;
// S calculation
			if ( max == 0 ) s=0;
			else s= 1 - (min/max);
// H calculation
			if (max==min) max++;	//Because when max=min it's undefined
			if (max == red) h = ( (60*(green-blue)) / (max-min) ) % 360;
			else if (max == green) h = ( (60*(blue - red)) / (max - min) + 120 ) % 360;
			else if (max == blue) h = ( (60*(red-green)) / (max-min) + 240 ) % 360;
			if (h < 0) h+= 360;
////
			new_data[y*step+x*channels] = h;
			new_data[y*step+x*channels+1] = s;
			new_data[y*step+x*channels+2] = v;
#ifdef HSV_DEBUG
			printf ("yo, h=%d, s=%d, v=%d, red=%d, green=%d, blue=%d, max=%d min=%d", h, s, v, red, green, blue, max, min);
			cvWaitKey(0);
#endif
		}
	}
	return new_data;
}

/*
 * This function gets an image and finds the center of weight for the element that "p" is a point on it using an algorithm 
 * that uses spreading recursively from that point on.
 * Retun Value: a CvPoint that is the calculate center of weight.
 */

CvPoint calculateCenter(IplImage* img,int *new_data , car car_to_check) {
	queue<CvPoint> que;
	int height = img->height;
	int width = img->width;
	int step = img->widthStep;
	CvPoint p = car_to_check.p;
	bool* used_arr = new bool[height*width];
	for (int i=0; i < (height*width); ++i) {
		used_arr[i]=false;	//initializing the array
	}
	int channels  = img->nChannels;
	uchar *data = (uchar *)img->imageData;
	
	uchar blue =car_to_check.blue ;
	uchar green =car_to_check.green ;
	uchar red = car_to_check.red ;
	int h =car_to_check.h ;
	int s =car_to_check.s ;
	int v =car_to_check.v ;
	
	que.push(p);
	CvPoint tmp;
	long int x_sum=0,y_sum=0,counter=0;
	while ( !que.empty() ){
		tmp = que.front();
		que.pop();
	
	if ( ( (   (abs(new_data[tmp.y*step+tmp.x*channels] - h) <=1) && (abs(new_data[tmp.y*step+tmp.x*channels+1] - s) <= 60)  ) || 
		 ((abs(data[tmp.y*step+tmp.x*channels+0] - blue) <=  50) && (abs(data[tmp.y*step+tmp.x*channels+1] - green) <= 50) && (abs(data[tmp.y*step+tmp.x*channels+2] - red) <= 35) )  
		 ) && (!(used_arr[tmp.y*width+tmp.x]))	) {

			used_arr[tmp.y*width+tmp.x] = true; 
			//data[tmp.y*step+tmp.x*channels+0] = 0; 
			//data[tmp.y*step+tmp.x*channels+1] = 0;
			//data[tmp.y*step+tmp.x*channels+2] = 255;
			x_sum+= tmp.x;
			y_sum+= tmp.y;
			counter++;
			if ((tmp.y < height-1) && (!(used_arr[(tmp.y+1)*width+tmp.x])) ) que.push(cvPoint(tmp.x, tmp.y+1));
			if ((tmp.y > 0) && (!(used_arr[(tmp.y-1)*width+tmp.x])) ) que.push(cvPoint(tmp.x, tmp.y-1));
			if ((tmp.x < width-1 ) && (!(used_arr[tmp.y*width+tmp.x+1])) ) que.push(cvPoint(tmp.x+1, tmp.y));
			if ((tmp.x > 0) && (!(used_arr[tmp.y*width+tmp.x-1])) ) que.push(cvPoint(tmp.x-1, tmp.y));
			}
	}
    delete[] used_arr;
	if ( counter ) {
	 x_sum = x_sum / counter;
	 y_sum = y_sum / counter;
	}else
	{
		x_sum = p.x;
		y_sum = p.y;
	}
	return  cvPoint(x_sum,y_sum);
}

bool isCarParked(CvPoint *PointArr){
	double center_x = (PointArr[0]).x;
	double center_y = (PointArr[0]).y;
	double dist=0;
	for(int i=1; i<NUMBER_OF_POINTS; ++i) {
		dist = sqrt( pow((PointArr[i].x - center_x),2) + pow((PointArr[i].y - center_y),2) );
		if (dist > OFFSET_RADIUS) return false;
	}
	return isThisParkingSlot(PointArr);
}

void checkIfCarEnter(int* new_img_hsv, int* first_frame_hsv,uchar* new_img_rgb, uchar* first_frame_rgb, int step, int channels,IplImage *img){
	static int last_counter = 0,car_id=0,frame=0;
	static bool car_in=false;
	int counter=0;
	bool *temp_arry = new bool [(ENTRANCE_RIGHT_DOWN.x-ENTRANCE_LEFT_UP.x)*(ENTRANCE_LEFT_UP.y-ENTRANCE_RIGHT_DOWN.y)];
	for ( int i=0; i < (ENTRANCE_RIGHT_DOWN.x-ENTRANCE_LEFT_UP.x)*(ENTRANCE_LEFT_UP.y-ENTRANCE_RIGHT_DOWN.y) ; ++i) temp_arry[i]=false;
	CvPoint enter_point=cvPoint(Xenter,Yenter);
	if ( frame > 0 ) frame--;
	for ( int x=ENTRANCE_LEFT_UP.x;x<ENTRANCE_RIGHT_DOWN.x;++x) 
		for ( int y = ENTRANCE_RIGHT_DOWN.y;y<ENTRANCE_LEFT_UP.y;++y) 
			if ( ((abs(new_img_hsv[y*step+x*channels] - first_frame_hsv[y*step+x*channels]) > 40000) || 
		 ((abs(new_img_rgb[y*step+x*channels+0] - first_frame_rgb[y*step+x*channels+0]) > 15) &&
		  (abs(new_img_rgb[y*step+x*channels+1] - first_frame_rgb[y*step+x*channels+1]) > 15) &&
		  (abs(new_img_rgb[y*step+x*channels+2] - first_frame_rgb[y*step+x*channels+2]) > 15) ) ) ){
				int temp_x=x-ENTRANCE_LEFT_UP.x;
				int temp_y=y-ENTRANCE_RIGHT_DOWN.y;
			    temp_arry[temp_y*(ENTRANCE_RIGHT_DOWN.x-ENTRANCE_LEFT_UP.x)+temp_x]=true;
				++counter;
	}
	
	if ( ((counter-last_counter) > 170 ) && last_counter && !car_in && frame == 0 ) {
	
		int sum_x=0,sum_y=0;
		for (int x=0; x< ENTRANCE_RIGHT_DOWN.x-ENTRANCE_LEFT_UP.x; ++x) {
			for (int y=0; y < ENTRANCE_LEFT_UP.y - ENTRANCE_RIGHT_DOWN.y; ++y) {
				if (temp_arry[y*(ENTRANCE_RIGHT_DOWN.x-ENTRANCE_LEFT_UP.x)+x]) {
					sum_x += x;
					sum_y += y;
				}
			}
		}
		enter_point=cvPoint((sum_x/counter)+ENTRANCE_LEFT_UP.x-50,(sum_y/counter)+ENTRANCE_RIGHT_DOWN.y+20);
		printf("Car number %d entered the lot!\n", car_id);
		carArrived( enter_point,car_id++ );
	    #ifdef UNIT_TEST
		num_car_enter++;
		#endif
		//cvCircle(img, enter_point , 4, cvScalar(0,255,0), 1);
		//delay = true;
		car_in=true;
		frame=20;
	}
	if ( ! ((counter-last_counter) > 160 ) ) car_in=false;
	last_counter=counter;
	delete[] temp_arry;
}

bool isCarExit(CvPoint *PointArr){
    if ( ( PointArr->x >= EXIT_LEFT_UP.x ) && ( PointArr->x <= EXIT_RIGHT_DOWN.x ) &&
		( PointArr->y >= EXIT_RIGHT_DOWN.y ) && ( PointArr->y <= EXIT_LEFT_UP.y ) ) return true;
	return false;
}

bool startTrackingCar(  CvCapture* capture) {
	int height, width, step, channels;
	uchar *data;
	cvNamedWindow("Top Cam", 1);
	cvGrabFrame(capture);
	IplImage *img;
	int *new_data ;
	img =  cvRetrieveFrame(capture);  // retrieve the captured frame
	cvShowImage("Top Cam", img);
	cvWaitKey(100);
	height = img->height;
	width = img->width;
	step = img->widthStep;
	channels  = img->nChannels;
	data = (uchar *)img->imageData;
	int* first_frame_hsv = ConvertFromRgbToHsv(img);
	uchar* first_frame_rgb = copyData(data,height,width,channels,step);
	int buff_count=1;
	Buffer buff(5,30);
	while ( cvGrabFrame(capture) ) { 
		img =  cvRetrieveFrame(capture);  // retrieve the captured frame
		buff.saveImage(img);
		list_of_cars.begin();
		for( vector<car>::iterator CurrCar = list_of_cars.begin(); CurrCar != list_of_cars.end(); CurrCar++ ){
			cvCircle(img, CurrCar->p , 20, cvScalar(0,255,0), 1);
		}	
		//cvRectangle(img, cvPoint(400,50) ,cvPoint(420,115) , cvScalar(0,255,0));
		data = (uchar *)img->imageData;
		new_data = ConvertFromRgbToHsv(img);
		list_of_cars.begin();
		//cvRectangle(img, ENTRANCE_LEFT_UP ,ENTRANCE_RIGHT_DOWN , cvScalar(0,0,255)); // enter Rectangle
		checkIfCarEnter(new_data, first_frame_hsv,data, first_frame_rgb, step,channels,img);
		for( vector<car>::iterator CurrCar = list_of_cars.begin(); CurrCar != list_of_cars.end(); CurrCar++ ){
			if (CurrCar->first_time) {
				CurrCar->blue = data[CurrCar->p.y*step+CurrCar->p.x*channels+0];
				CurrCar->green = data[CurrCar->p.y*step+CurrCar->p.x*channels+1];
				CurrCar->red = data[CurrCar->p.y*step+CurrCar->p.x*channels+2] ;
				CurrCar->h = new_data[CurrCar->p.y*step+CurrCar->p.x*channels+0];
				CurrCar->s = new_data[CurrCar->p.y*step+CurrCar->p.x*channels+1];
				CurrCar->v = new_data[CurrCar->p.y*step+CurrCar->p.x*channels+2] ;
				CurrCar->first_time= false;
			}	
			CurrCar->p=calculateCenter(img, new_data , *CurrCar );

			CurrCar->PointArr[CurrCar->IndexToPointArr++]=CurrCar->p;
		    if (CurrCar->IndexToPointArr == NUMBER_OF_POINTS) {
				CurrCar->check_park = true;
				CurrCar->IndexToPointArr = 0;
			}
			if ( (! CurrCar->is_park ) && isCarParked(CurrCar->PointArr) ) {
				printf("car %d parked \n",CurrCar->licensePlate);
				Protocol::tc2db_parked (*Top_Cam_Sock,(unsigned long )CurrCar->licensePlate, (unsigned int)CurrCar->p.x,(unsigned int)CurrCar->p.y);
				#ifdef UNIT_TEST
				num_car_park++; // for testing
				#endif
				CurrCar->is_park = true;
			}
			if (  CurrCar->is_park  && (!isCarParked(CurrCar->PointArr)) ) {
				printf("car %d unparked \n",CurrCar->licensePlate);
				Protocol::tc2db_unparked (*Top_Cam_Sock,(unsigned int)CurrCar->p.x,(unsigned int)CurrCar->p.y);
				#ifdef UNIT_TEST
				num_car_unpark++; // for testing
				#endif
				string videoLocation = buff.createVideoLog( buff_count++ );
				Protocol::tc2db_video(*Top_Cam_Sock,(unsigned long )CurrCar->licensePlate,videoLocation);
				CurrCar->is_park = false;
			}
			cvCircle(img, CurrCar->p , 5, cvScalar(0,255,0), 1);
			if ( isCarExit(CurrCar->PointArr) ){
				printf("car %d exited \n",CurrCar->licensePlate);
				Protocol::tc2db_exited(*Top_Cam_Sock,(unsigned long )CurrCar->licensePlate);
				#ifdef UNIT_TEST
				num_car_exit++; // for testing
				#endif
				list_of_cars.erase(CurrCar );
				if ( list_of_cars.begin() == list_of_cars.end() ) break;
				CurrCar = list_of_cars.begin();
			}
		}
		//cvRectangle(img, EXIT_LEFT_UP ,EXIT_RIGHT_DOWN , cvScalar(0,0,255)); // exit Rectangle
		//cvRectangle(img, PARK_LEFT_UP , PARK_RIGHT_DOWN , cvScalar(0,0,255));
		delete[] new_data;
		cvShowImage("Top Cam", img);
		if ( delay ) {
			cvWaitKey(10000);
			delay = false;
		}
			else cvWaitKey(10);
	}
	cvWaitKey(10000);
	delete[] first_frame_hsv;
	delete[] first_frame_rgb;
	cvDestroyWindow("Top Cam");
	return true;
}


void initParkinglot( void *captureTop , SocketClient *sock){		// == main_top
	CvCapture* capture = (CvCapture *) captureTop;
	Top_Cam_Sock = sock;
	startTrackingCar(capture);
	cvReleaseCapture(&capture);
}




